﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Kinect.Gesture;

namespace Kinect.Toolbox
{
    public class PushPullGestureDetector : GestureDetector
    {
        public float AvailableMinimalLength { get; set; }
        public float ForbidMaximalLength { get; set; }
        public int MininalDuration { get; set; }
        public int MaximalDuration { get; set; }

        public PushPullGestureDetector()
        {
            AvailableMinimalLength = 0.4f;
            ForbidMaximalLength = 0.2f;
            MininalDuration = 250;
            MaximalDuration = 1500;
        }

        private int entryIndex = -1;

        protected bool ScanPositions(Func<Vector3Struct, Vector3Struct, bool> forbidAxisFunc, Func<Vector3Struct, Vector3Struct, bool> directionFunction,Func<Vector3Struct, Vector3Struct, bool> lengthFunction, int minTime, int maxTime)
        {

            int start = 0;
            int entryCount = 0;
            entryIndex = -1;
            for (int index = 1; index < Entries.Count - 1; index++)
            {

                Skeleton firstSkeleton = Entries[0].Skeleton;
                SkeletonPoint firstLeftHand = firstSkeleton.Joints[JointType.HandLeft].Position,
                    firstRightHand = firstSkeleton.Joints[JointType.HandRight].Position;
                Vector3Struct firstLeftVec3 = firstLeftHand.ToVector3(),
                    firstRightVec3 = firstRightHand.ToVector3();

                Skeleton scanSkeleton = Entries[index].Skeleton;
                Vector3Struct scanLeftVec3 = scanSkeleton.Joints[JointType.HandLeft].Position.ToVector3(),
                    scanRightVec3 = scanSkeleton.Joints[JointType.HandRight].Position.ToVector3();

                Skeleton nextScanSkeleton = Entries[index + 1].Skeleton;
                Vector3Struct nextScanLeftVec3 = nextScanSkeleton.Joints[JointType.HandLeft].Position.ToVector3(),
                    nextScanRightVec3 = nextScanSkeleton.Joints[JointType.HandRight].Position.ToVector3();


                if (!forbidAxisFunc(firstLeftVec3, scanLeftVec3) || !directionFunction(scanLeftVec3, nextScanLeftVec3))
                {
                    start = index;
                }
                Skeleton startAvailableSkeleton = Entries[start].Skeleton;
                Vector3Struct startAvailableLeftVec3 = startAvailableSkeleton.Joints[JointType.HandLeft].Position.ToVector3();

                if (lengthFunction(scanLeftVec3, startAvailableLeftVec3))
                {
                    double totalMilliseconds = (Entries[index].Time - Entries[start].Time).TotalMilliseconds;
                    if (totalMilliseconds >= minTime && totalMilliseconds <= maxTime)
                    {
                        entryIndex = index;
                        entryCount++;
                    }
                }
            }
            if (entryCount > 0)
                return true;
            return false;

        }

        protected override void LookForGesture()
        {
            //Push
            if (ScanPositions((p1, p2) =>   Math.Abs(p2.X - p1.X) < ForbidMaximalLength /*&& Math.Abs(p2.Y - p1.Y) < ForbidMaximalLength*/, // x and y length
                (p1, p2) => p2.Z - p1.Z < 0.01f, // Progression to -z
                (p1, p2) => Math.Abs(p2.Z - p1.Z) > AvailableMinimalLength, // Length
                MininalDuration, MaximalDuration)) // Duration
            {
                RaiseGestureDetected(new GestureEventArgs(E_GestureDescrition.Push,JointType.HandLeft));

                return;
            }
            //Pull
            if (ScanPositions((p1, p2) =>  Math.Abs(p2.X - p1.X) < ForbidMaximalLength /*&& Math.Abs(p2.Y - p1.Y) < ForbidMaximalLength*/, // Height
                (p1, p2) => p2.Z - p1.Z > -0.01f, // Progression to +z
                (p1, p2) => Math.Abs(p2.Z - p1.Z) > AvailableMinimalLength, // Length
                MininalDuration, MaximalDuration)) // Duration
            {

                RaiseGestureDetected(new GestureEventArgs(E_GestureDescrition.Pull, JointType.HandLeft) );

                return;
            }
        }
    }
}
